/***************************************************************************************
 * 
 * 本文件接口需要进行安全码校验
 * 
*/
#pragma once
#include <string.h>
#include "motion/rtcmds.h"
#include "motion/safezone.h"
#include "motion/motion.h"
#include "safety/safety_type.h"

#define SAFETY_CMD_CLASS_NAME(x) CMD_CLASS_NAME(x) "[SAFETY CMD]"

class SafetyCmd : public mot::MultiRobCmd
{
public:
    SafetyCmd() = default;
    virtual ~SafetyCmd() = default;
    virtual int active_callback();
};

class SafetyCmdSetCollisionSensitivity : public SafetyCmd
{
public:
    SafetyCmdSetCollisionSensitivity(int grp_id, const int* joint_sensitivity)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(SafetyCmdSetCollisionSensitivity);
        for (int i = 0; i < 9; i++) { joint_sensitivity_[i] = joint_sensitivity[i]; }
    }
    ~SafetyCmdSetCollisionSensitivity() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    int joint_sensitivity_[9];
};

class CmdSetForceLimitValue : public SafetyCmd
{
public:
    CmdSetForceLimitValue(int grp_id, int force_limit_value)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetForceLimitValue);
        force_limit_value_ = force_limit_value;
    };
    ~CmdSetForceLimitValue() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    int force_limit_value_;
};

class CmdSetCollisionDetectionEnable : public SafetyCmd
{
public:
    CmdSetCollisionDetectionEnable(int grp_id, int collision_detection_enable)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetCollisionDetectionEnable);
        collision_detection_enable_ = collision_detection_enable;
    };
    ~CmdSetCollisionDetectionEnable() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    int collision_detection_enable_;
};

class CmdSetRunMode : public SafetyCmd
{
public:
    CmdSetRunMode(int grp_id, int mode, int serial_num)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetRunMode);
        mode_ = mode;
        serial_num_ = serial_num;
    };
    ~CmdSetRunMode() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int mode_;
    int serial_num_;
};

class CmdSetFunctionSaftyPara : public SafetyCmd
{
public:
    CmdSetFunctionSaftyPara(int grp_id,
                            const double* robot_momentum_limit,
                            const double* robot_endforce_limit,
                            double robot_velocity_limit,
                            const double* robot_power_limit)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetFunctionSaftyPara);
        for (int i = 0; i < 2; i++) { robot_momentum_limit_[i] = robot_momentum_limit[i]; }
        for (int i = 0; i < 2; i++) { robot_endforce_limit_[i] = robot_endforce_limit[i]; }
        robot_velocity_limit_ = robot_velocity_limit;
        for (int i = 0; i < 2; i++) { robot_power_limit_[i] = robot_power_limit[i]; }
    };
    ~CmdSetFunctionSaftyPara() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double robot_momentum_limit_[2];
    double robot_endforce_limit_[2];
    double robot_velocity_limit_;
    double robot_power_limit_[2];
};

class CmdSetJointMinLimits : public SafetyCmd
{
public:
    CmdSetJointMinLimits(int grp_id, int joint_num, double minLimit)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetJointMinLimits);
        joint_num_ = joint_num;
        minLimit_ = minLimit;
    };
    ~CmdSetJointMinLimits() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    int joint_num_;
    double minLimit_;
};

class CmdSetJointMaxLimits : public SafetyCmd
{
public:
    CmdSetJointMaxLimits(int grp_id, int joint, double maxLimit)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetJointMaxLimits);
        joint_num_ = joint;
        maxLimit_ = maxLimit;
    };
    ~CmdSetJointMaxLimits() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    int joint_num_;
    double maxLimit_;
};

class CmdSetRobotPowerLimit : public SafetyCmd
{
public:
    CmdSetRobotPowerLimit(int grp_id, double robotPowerLimit)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetRobotPowerLimit);
        robotPowerLimit_ = robotPowerLimit;
    };
    ~CmdSetRobotPowerLimit() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double robotPowerLimit_;
};

class CmdSetTcpVelLimit : public SafetyCmd
{
public:
    CmdSetTcpVelLimit(int grp_id, double tcpVelLimit)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetTcpVelLimit);
        tcpVelLimit_ = tcpVelLimit;
    };
    ~CmdSetTcpVelLimit() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double tcpVelLimit_;
};

class CmdSetDragTcpSpeedLimit : public SafetyCmd
{
public:
    CmdSetDragTcpSpeedLimit(int grp_id, double dragTcpSpeedLimit)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetDragTcpSpeedLimit);
        dragTcpSpeedLimit_ = dragTcpSpeedLimit;
    };
    ~CmdSetDragTcpSpeedLimit() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double dragTcpSpeedLimit_;
};

class CmdSetStoppingDist : public SafetyCmd
{
public:
    CmdSetStoppingDist(int grp_id, double stoppingDist)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetStoppingDist);
        stoppingDist_ = stoppingDist;
    };
    ~CmdSetStoppingDist() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double stoppingDist_;
};

class CmdSetStoppingTime : public SafetyCmd
{
public:
    CmdSetStoppingTime(int grp_id, double stoppingTime)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetStoppingTime);
        stoppingTime_ = stoppingTime;
    };
    ~CmdSetStoppingTime() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double stoppingTime_;
};

class CmdSetStoppingDistReduce : public SafetyCmd
{
public:
    CmdSetStoppingDistReduce(int grp_id, double stopping_dist_reduce)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetStoppingDistReduce);
        stopping_dist_reduce_ = stopping_dist_reduce;
    };
    ~CmdSetStoppingDistReduce() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double stopping_dist_reduce_;
};

class CmdSetStoppingTimeReduce : public SafetyCmd
{
public:
    CmdSetStoppingTimeReduce(int grp_id, double stopping_time_reduce)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetStoppingTimeReduce);
        stopping_time_reduce_ = stopping_time_reduce;
    };
    ~CmdSetStoppingTimeReduce() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double stopping_time_reduce_;
};

class CmdSetElbowVelLimitReduce : public SafetyCmd
{
public:
    CmdSetElbowVelLimitReduce(int grp_id, double elbowVelLimitReduce)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetElbowVelLimitReduce);
        elbowVelLimitReduce_ = elbowVelLimitReduce;
    };
    ~CmdSetElbowVelLimitReduce() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double elbowVelLimitReduce_;
};

class CmdSetTcpVelLimitReduce : public SafetyCmd
{
public:
    CmdSetTcpVelLimitReduce(int grp_id, double tcpVelLimitReduce)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetTcpVelLimitReduce);
        tcpVelLimitReduce_ = tcpVelLimitReduce;
    };
    ~CmdSetTcpVelLimitReduce() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double tcpVelLimitReduce_;
};

class CmdSetMomentumLimitReduce : public SafetyCmd
{
public:
    CmdSetMomentumLimitReduce(int grp_id, double momentumLimitReduce)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetMomentumLimitReduce);
        momentumLimitReduce_ = momentumLimitReduce;
    };
    ~CmdSetMomentumLimitReduce() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double momentumLimitReduce_;
};

class CmdSetRobotPowerLimitReduce : public SafetyCmd
{
public:
    CmdSetRobotPowerLimitReduce(int grp_id, double robotPowerLimitReduce)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetRobotPowerLimitReduce);
        robotPowerLimitReduce_ = robotPowerLimitReduce;
    };
    ~CmdSetRobotPowerLimitReduce() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double robotPowerLimitReduce_;
};

class CmdSetMomentumLimit : public SafetyCmd
{
public:
    CmdSetMomentumLimit(int grp_id, double momentumLimit)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetMomentumLimit);
        momentumLimit_ = momentumLimit;
    };
    ~CmdSetMomentumLimit() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double momentumLimit_;
};

class CmdSetJointVelLimit : public SafetyCmd
{
public:
    CmdSetJointVelLimit(int grp_id, int joint_num, double vel)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetJointVelLimit);
        joint_num_ = joint_num;
        vel_ = vel;
    };
    ~CmdSetJointVelLimit() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    int joint_num_;
    double vel_;
};

class CmdSetJointErrLimit : public SafetyCmd
{
public:
    CmdSetJointErrLimit(int grp_id, int joint_num, double errPosLimit)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetJointErrLimit);
        joint_num_ = joint_num;
        errPosLimit_ = errPosLimit;
    };
    ~CmdSetJointErrLimit() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    int joint_num_;
    double errPosLimit_;
};

class CmdTaskInitFinish : public SafetyCmd
{
public:
    CmdTaskInitFinish(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdTaskInitFinish);
    };
    ~CmdTaskInitFinish() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };
};

class CmdInitSafetyCrc : public SafetyCmd
{
public:
    CmdInitSafetyCrc(int grp_id, double checksum)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdInitSafetyCrc);
        checksum_ = checksum;
    };
    ~CmdInitSafetyCrc() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double checksum_;
};

class CmdSetSafeZoneElbowWristSphere : public SafetyCmd
{
public:
    CmdSetSafeZoneElbowWristSphere(int grp_id, double safezone_elbow_radius, double safezone_elbow_offest)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetSafeZoneElbowWristSphere);
        safezone_elbow_radius_ = safezone_elbow_radius;
        safezone_elbow_offest_ = safezone_elbow_offest;
    };
    ~CmdSetSafeZoneElbowWristSphere() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double safezone_elbow_radius_;
    double safezone_elbow_offest_;
};

class CmdSetSafeZone : public SafetyCmd
{
public:
    CmdSetSafeZone(int grp_id, SafeZone zone)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetSafeZone);
        zone_ = zone;
    };
    ~CmdSetSafeZone() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    SafeZone zone_;
};

class CmdSetSafeZoneEnable : public SafetyCmd
{
public:
    CmdSetSafeZoneEnable(int grp_id, int enable)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetSafeZoneEnable);
        enable_ = enable;
    };
    ~CmdSetSafeZoneEnable() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int enable_;
};

class CmdSetOneKeyOp : public SafetyCmd
{
public:
    CmdSetOneKeyOp(int grp_id, int autoprogram, int onekeyenable)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetOneKeyOp);
        autoprogram_ = autoprogram;
        onekeyenable_ = onekeyenable;
    };
    ~CmdSetOneKeyOp() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int autoprogram_;
    int onekeyenable_;
};

class CmdServoSetParam : public SafetyCmd
{
public:
    CmdServoSetParam(int grp_id, ServoParam param)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdServoSetParam);
        param_ = param;
    };
    ~CmdServoSetParam() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    ServoParam param_;
};

class CmdSetAutoWork : public SafetyCmd
{
public:
    CmdSetAutoWork(int grp_id, const char* filename, int enable)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetAutoWork);

        enable_ = enable;
        strncpy(filename_, filename, 256);
    };
    ~CmdSetAutoWork() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    char filename_[256];
    int enable_;
};

class CmdSetAttitudeSafety : public SafetyCmd
{
public:
    CmdSetAttitudeSafety(int grp_id, zuc_set_attitude_safety setattitudesafety)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetAttitudeSafety);
        setattitudesafety_ = setattitudesafety;
    };
    ~CmdSetAttitudeSafety() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    zuc_set_attitude_safety setattitudesafety_;
};

class CmdSetCollisionOption : public SafetyCmd
{
public:
    CmdSetCollisionOption(int grp_id, CollisionOption clsnOpt)
    {
        group_id_ = grp_id;
        cmd_name_ = SAFETY_CMD_CLASS_NAME(CmdSetCollisionOption);
        clsnOpt_ = clsnOpt;
    };
    ~CmdSetCollisionOption() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    CollisionOption clsnOpt_;
};